{
 "nbformat": 4,
 "nbformat_minor": 2,
 "metadata": {
  "language_info": {
   "name": "python",
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "version": "3.7.7-final"
  },
  "orig_nbformat": 2,
  "file_extension": ".py",
  "mimetype": "text/x-python",
  "name": "python",
  "npconvert_exporter": "python",
  "pygments_lexer": "ipython3",
  "version": 3,
  "kernelspec": {
   "name": "python37764bitcvconda2603be1e67004bc6b46dca87e540e114",
   "display_name": "Python 3.7.7 64-bit ('cv': conda)"
  }
 },
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "为了获得相机的内外参数矩阵，需要对相机进行标定。  \n",
    "首先要将目标所在的世界坐标系转换为相机坐标系。  \n",
    "然后就能通过小孔成像的原理，将相机坐标系中的目标转换为图像坐标系。  \n",
    "而通过相机标定就是为了求得准确的相机内参矩阵跟外参矩阵。  \n",
    "相机标定通过Zhang方法进行，对目标进行不同的至少17张pose图，每张图最好能够相对相机的不同位姿有区别，这样能尽可能的求得最佳的外参数矩阵。  \n",
    "外参数矩阵通过不同pose可以获得。  \n",
    "内参数矩阵通过转换公式可以求得。由于现实中光线，空气水分，尘埃等可能产生光线折射的情况，会导致畸变。即使相同型号的相机、不同的厂商，玻璃做工等不同，造成镜面曲面不一样的。产生畸变。  \n",
    "因此还需要考虑畸变系数。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "使用小米mix3手机进行拍摄，获取棋盘格图片  \n",
    "从拍摄的图不能看出明显的畸变，说明相机的畸变系数还可以，畸变误差应该较小  \n",
    "![title](images/camera1.jpeg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import glob\n",
    "# 设置终止条件，迭代30次或变动小于0.001\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "# 生成42×3的矩阵，用来保存棋盘图中6*9个内角点的3D坐标，也就是物体点坐标 \n",
    "objp = np.zeros((6*9, 3), np.float32)\n",
    "# 通过np.mgrid生成对象的xy坐标点\n",
    "# 最终得到的objp为(0,0,0), (1,0,0), (2,0,0) ,..., (6,9,0) \n",
    "objp[:, :2] = np.mgrid[0:6, 0:9].T.reshape(-1, 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# path为正则匹配、或者固定路径\n",
    "def getObjImgPointsWithPath(path):\n",
    "    obj_points = [] # 用于保存物体点 \n",
    "    img_points = [] # 用于保存图像点\n",
    "    shape = 0\n",
    "    # 返回当前目录所有匹配的jpeg图片 \n",
    "    images = glob.glob(path)\n",
    "    for fname in images: # 读取图片\n",
    "        img = cv2.imread(fname)\n",
    "        # 转为灰度图\n",
    "        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "        if shape==0:\n",
    "            shape=gray.shape\n",
    "        # 寻找棋盘图的内角点位置\n",
    "        ret, corners = cv2.findChessboardCorners(gray, (6,9), None) # 棋盘格内的内角点行列数为6*9\n",
    "        # 如果找到棋盘图的所有内角点 \n",
    "        if ret == True:\n",
    "            obj_points.append(objp)\n",
    "            # 亚像素级角点检测，在角点检测中精确化角点位置\n",
    "            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) #（11，11）表示搜索窗大小为11*2+1=23 （-1，-1）表示没零区域\n",
    "            img_points.append(corners2)\n",
    "    return obj_points, img_points, shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "相机内参数矩阵： [[1.09676242e+03 0.00000000e+00 7.15752274e+02]\n [0.00000000e+00 1.09684605e+03 5.46891179e+02]\n [0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n畸变矩阵： [[ 2.93135585e-01 -1.33809907e+00  3.65134455e-04 -1.21177442e-03\n   1.72174449e+00]]\n旋转向量： [array([[ 0.13169239],\n       [ 0.09976689],\n       [-1.66746502]]), array([[ 0.04623065],\n       [ 0.04276969],\n       [-0.83220797]]), array([[ 0.26668525],\n       [ 0.02927019],\n       [-1.46454625]]), array([[ 0.20672946],\n       [ 0.09834462],\n       [-1.80215876]]), array([[ 0.00988484],\n       [ 0.0450461 ],\n       [-1.40980409]]), array([[ 0.13300891],\n       [ 0.13787635],\n       [-1.61971859]]), array([[ 0.05266594],\n       [ 0.04234529],\n       [-1.56410936]]), array([[ 0.09659349],\n       [ 0.11780047],\n       [-1.85534641]]), array([[ 0.19281659],\n       [ 0.06104052],\n       [-1.3883812 ]]), array([[ 0.17867038],\n       [ 0.08294269],\n       [-1.69068454]]), array([[-0.00985191],\n       [ 0.08188786],\n       [-1.63491268]]), array([[ 0.19958013],\n       [ 0.19200574],\n       [-1.56760501]]), array([[ 0.15663809],\n       [ 0.00490074],\n       [-1.53462748]]), array([[-0.01456131],\n       [ 0.0444428 ],\n       [-1.63553829]]), array([[ 2.98077249e-01],\n       [ 9.72551895e-04],\n       [-1.60245680e+00]]), array([[ 0.19336361],\n       [ 0.06911656],\n       [-1.56912555]])]\n位移向量： [array([[-3.14416848],\n       [ 3.02562421],\n       [12.29085149]]), array([[-5.54856692],\n       [-1.00926243],\n       [13.35656901]]), array([[-5.08189415],\n       [ 2.58045641],\n       [10.54625518]]), array([[-2.96830852],\n       [ 3.70103417],\n       [11.00028724]]), array([[-5.93554006],\n       [ 2.25744913],\n       [11.4735747 ]]), array([[-2.75238859],\n       [ 2.13201421],\n       [10.72258994]]), array([[-3.30670437],\n       [ 3.85027867],\n       [10.73845841]]), array([[-2.35152508],\n       [ 3.5607915 ],\n       [11.3066659 ]]), array([[-4.6367243 ],\n       [ 2.16286238],\n       [10.65541174]]), array([[-3.46266946],\n       [ 3.61805582],\n       [10.95861975]]), array([[-5.92762105],\n       [ 4.26406649],\n       [13.05963938]]), array([[-3.36359087],\n       [ 1.70802112],\n       [10.75820641]]), array([[-5.08653287],\n       [ 3.18540022],\n       [10.41075486]]), array([[-4.92556105],\n       [ 3.775922  ],\n       [11.24542651]]), array([[-4.74158059],\n       [ 3.0586999 ],\n       [10.56029851]]), array([[-4.77731382],\n       [ 2.00755543],\n       [10.37024251]])]\n"
    }
   ],
   "source": [
    "# 获取图片角点坐标\n",
    "obj_points, img_points, shape = getObjImgPointsWithPath('./images/*.jpeg')\n",
    "# 相机标定\n",
    "_, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, shape, None, None)\n",
    "print('相机内参数矩阵：', mtx)\n",
    "print('畸变矩阵：', dist)\n",
    "print('旋转向量：', rvecs)\n",
    "print('位移向量：', tvecs)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可以得到相机内参数fx=1096 fy=1096 u0=715 v0=546  \n",
    "u0 v0 正好是图像一半像素大小附近范围内，属于正常数值  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "mean error:  0.0775207199162505\n"
    }
   ],
   "source": [
    "# 计算重投影误差\n",
    "total_error = 0\n",
    "for i in range(len(obj_points)):\n",
    "    img_points2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)\n",
    "    error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2)/len(img_points2)\n",
    "    total_error += error\n",
    "print(\"mean error: \", total_error/len(obj_points))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 位姿估计"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": "./pnp_images/img4.jpeg\nR： [[-0.00671587]\n [-0.00408298]\n [-1.57059587]]\nT： [[-9.70641859]\n [10.01772293]\n [29.15474999]]\n./pnp_images/img2.jpeg\nR： [[-0.12562857]\n [ 0.15549093]\n [-1.54039926]]\nT： [[-6.22654875]\n [-0.74495401]\n [27.11517921]]\n./pnp_images/img3.jpeg\nR： [[-0.02321483]\n [ 0.08073591]\n [-1.56631995]]\nT： [[-9.21297927]\n [ 5.64585808]\n [29.73846709]]\n./pnp_images/img1.jpeg\nR： [[ 0.04361054]\n [ 0.1698355 ]\n [-1.52769904]]\nT： [[-11.86505426]\n [ -1.26497201]\n [ 27.17010947]]\n"
    }
   ],
   "source": [
    "# 获取需要位姿估计的图片\n",
    "images = glob.glob('./pnp_images/*.jpeg')\n",
    "for fname in images: # 读取图片\n",
    "    img = cv2.imread(fname)\n",
    "    # 转为灰度图\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    # 寻找棋盘图的内角点位置\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (6,9), None) # 棋盘格内的内角点行列数为6*9\n",
    "    # 如果找到棋盘图的所有内角点 \n",
    "    if ret == True:\n",
    "        ret2, rv, tv = cv2.solvePnP(objp, corners, mtx, dist, np.array(rvecs), np.array(tvecs))\n",
    "        print(fname)\n",
    "        print('R：', rv)\n",
    "        print('T：', tv)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "拍摄的时候相机基本和棋盘格平行，因此旋转角度都差不多，和输出的结果近似。  \n",
    "而平移主要是对x，y进行，z也就是距离由于手抖产生了一些误差，不过基本不大。  \n",
    "x，y所对应的坐标所在的位置也跟拍摄的图片位置符合。  "
   ]
  }
 ]
}